#include "EmergencyStopDisabledEvent.hpp"
#include "EmergencyStopState.hpp"
#include "HardwareController.hpp"
#include "Robot.hpp"

EmergencyStopState::EmergencyStopState() :
	previousState(Robot::getInstance().getState())
{ }

void EmergencyStopState::restorePreviousState()
{
	//Restore previous state:

	Robot::getInstance().changeState(previousState);
}

void EmergencyStopState::handleEvent(Event& event)
{
	//Only handle emergency stop disabled events, ignore everything else:
	if (event.getEventType() == EMERGENCYSTOPDISABLED)
		restorePreviousState();
}

void EmergencyStopState::initState()
{
	//Invoke emergency state on hardware:

	//Stop engine
	Robot::getInstance().getEngine().stop();

	//Blink lights while emergencystop is active
	while (HardwareController::getHardwareController().pollEmergencyStop())
	{
		HardwareController::getHardwareController().wait(50);
		HardwareController::getHardwareController().activateLED();
		HardwareController::getHardwareController().wait(50);
		HardwareController::getHardwareController().disableLED();
	}

	//Continue work
	restorePreviousState();
}

State::StateType EmergencyStopState::getStateType()
{
	return State::EMERGENCYSTOP;
}
